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ON-LINE  PATH  GENERATION  AND  TRACKING  FOR 
HIGH-SPEED  WHEELED  AUTONOMOUS  VEHICLES 
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Executive  Summary 

The  main  objective  of  this  work  has  been  to  develop  control  algorithms  capable  of  steering  wheeled 
ground  vehicles  autonomously,  by  avoiding  obstacles  and  threats,  while  moving  in  high-speed  over 
rough/uneven  terrain.  It  is  assumed  that  the  vehicle  has  only  limited  onboard  computational  re¬ 
sources.  The  ultimate  objective  is  to  build  an  autonomous  vehicle  that  operates  similarly  to  the 
way  expert  human  (eg.,  rally  race)  drivers  do.  The  emphasis  on  high-speed  is  motivated  by  the 
advantages  in  terms  of  reaction  time,  minimization  of  exposure  to  danger  for  vehicles  operating  in 
enemy  territory,  increase  of  supply  line  capacity  and/or  delivery  time  reduction  of  materiel,  etc. 
The  most  noteworthy  accomplishments  from  this  research  include: 

•  A  mathematical  formalization  for  the  description  and  generation  of  aggressive  maneuvers  used  by 
expert  rally  racing  drivers  (e.g.,  trail  braking  and  pendulum  turn)  using  trajectory  optimization. 
We  have  thus  encapsuled  expert  human  driving  knowledge  which  can  be  subsequently  used  in 
creating  path  and  motion  primitives  for  on-line  implementation  (papers  [1,  2,  3,  4,  5,  6,  7,  8,  9, 
10,  11,  12,  13]). 

•  The  development  of  a  fast  multi-resolution  algorithm  for  the  on-line  path  generation  in  a  partially 
known,  changing  environment.  The  algorithm  makes  use  of  a  multi-resolution  decomposition  of 
the  map  that  represents  the  environment.  Obstacle  boundaries  are  resolved  with  high  fidelity 
only  when  (or  if)  the  vehicle  gets  close  to  these  obstacles.  Far  away  obstacles  are  inconsequential 
for  the  short-term  planning  and  hence  these  are  approximated  with  lower  fidelity.  We  there¬ 
fore  create  a  natural  way  of  prioritizing  threats  and  obstacles  in  a  numerically  efficient  manner, 
without  sacrificing  accuracy.  We  can  thus  close  the  gap  between  strategic  (decision-making)  and 
tactical  (reflexive)  layers  (papers  [14,  15,  16,  17]). 

•  A  kinodynamic  motion-planning  algorithm  that  blends  together  the  path-planning  and  motion¬ 
planning  control  layers  of  the  problem,  allowing  the  generation  of  kinematically  feasible  trajec¬ 
tories  directly  from  the  geometric  path  planner.  This  is  achieved  by  incorporating  curvature 
information  of  the  allowable  paths  via  a  novel  modification  of  Dijkstra’s  algorithm  that  treats 
histories  of  cells  or  by  creating  suitable  path/trajectory  primitives  and  by  combining  them  in 
such  way  that  respects  the  kinodynamic  constraints  of  the  problem  (papers  [18,  19,  20,  21]). 

The  support  of  this  award  enabled  one  student  to  obtain  his  M.S.  degree  [22]  (E.  Bakolas), 
one  student  to  complete  his  Ph.D.  degree  [23]  (E.  Velenis).  Two  more  students  were  supported 
during  their  doctoral  studies  (R.  Cowlagi  and  E.  Bakolas).  The  doctoral  thesis  of  E.  Velenis  re¬ 
ceived  the  Luther  Long  Award  in  Engineering  Mechanics  for  the  best  doctoral  thesis  at  Georgia 
Tech  (2006).  The  paper  “Shortest  Distance  Problems  in  Graphs  Using  History-Dependent  Transi¬ 
tion  Costs  with  Application  to  Kinodynamic  Path  Planning”  written  by  R.  Cowlagi  received  the 
best  student  paper  award  in  the  2009  American  Control  Conference  held  from  June  10-12,  2009  in 
St.  Louis,  Missouri.  Four  (4)  archival  journal  publications  (two  published  and  two  under  review) 
and  seventeen  (17)  conference  papers  (all  in  peer-reviewed  conferences)  document  the  results  of 
this  work  in  the  open  literature. 
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1  Introduction 

1.1  Motivation  and  Problem  Statement 

Study  after  study  has  shown  the  benefits  of  speed  in  military  operations,  both  in  terms  of  engaging 
the  enemy,  avoiding  ambushes  and  successfully  completing  the  mission.  The  US  Army  has  been 
promoting  the  large-scale  deployment  of  autonomous  or  semi-autonomous  vehicles  in  the  battle¬ 
field,  either  as  part  of  the  support  supply  chain  (convoys)  or  as  active  participants  in  the  battle 
zone.  It  is  clear  even  to  the  non-expert  that  these  vehicles  will  not  survive  for  long  in  the  battlefield 
if  they  are  restrained  to  move  and  maneuver  using  the  modest  speeds  demonstrated  to  date. 

A  class  of  vehicles  we  envision  to  be  completely  automated  in  the  future  are  ground  wheeled 
vehicles  (Fig.  1(a))  that  operate  in  hostile  off-road  environments  (e.g.,  battlefields).  A  typical  mis¬ 
sion  would  be  to  drive  the  vehicle  from  point  A  to  point  B,  avoid  any  obstacles,  while  minimizing 
exposure  to  danger;  see  Fig.  1(b).  In  general,  minimization  of  the  exposure  to  danger  involves 
driving  in  minimum  time  or  with  maximum  average  velocity. 


(a)  (b) 


Figure  1:  (a)  Autonomous  military  off-road  vehicles  will  exhibit  a  high  degree  of  tactical  mobility, 
while  eliminating  the  risk  of  loss  of  human  lives;  (b)  A  typical  mission  scenario  may  involve  an 
autonomous  vehicle  entering  a  hazardous  area  to  perform  its  mission  objective,  while  avoiding 
obstacles  and/or  minimizing  its  exposure  to  enemy  threats  and  countermeasures. 

In  order  to  increase  manifold  the  survivability  of  these  vehicles  (thus  also  increasing  the  success 
rates  of  the  mission)  a  new  level  of  maneuverability  is  required.  These  vehicles  will  have  to  move  in 
high-speed,  inside  a  dynamically  changing  environment  and  will  have  to  make  split-second  decisions 
with  limited  onboard  computational  resources.  Our  long-term  research  objective  is  to  make  this 
vision  a  reality. 

2  Summary  of  Most  Noteworthy  Research  Accomplishments 

The  main  goal  of  this  report  is  to  summarize  the  results  obtained  under  this  research  program. 
Since  most  of  the  technical  results  have  appeared  or  will  soon  appear  in  over  21  archival  journal 
and  conference  publications,  below  we  only  provide  a  summary  of  these  results  and  remark  on  their 
significance.  Further  details  can  be  found  in  the  List  of  Publications  at  the  end  of  this  report. 

2.1  Aggressive/Technical  Driving  for  High  Speed  Vehicles 

Following  up  on  our  previous  work  in  this  area,  in  [2,  4]  we  investigated  the  problem  of  generating 
near-minimum  time  velocity  profiles  for  a  vehicle  along  a  specified  path  subject  to  acceleration 
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constraints,  similar  to  those  arising  from  the  friction  ellipse  between  the  tires  of  the  vehicle  and  the 
ground.  Initially  we  adopted  a  point  mass  parametrization  of  the  vehicle.  Using  optimal  control 
theory  we  completely  solved  this  problem,  characterizing  the  optimal  control  switching  strategy  in 
a  mathematically  rigorous  manner.  In  generating  the  optimal  velocity  profile,  several  undesirable 
cases,  where  loss  of  controllability  occurs,  and  which  had  been  neglected  in  the  literature,  were  also 
dealt  with.  A  receding  horizon  implementation  was  proposed  for  the  on-line  implementation  of  the 
velocity  optimizer  [2,  5].  Robustness  of  the  receding  horizon  algorithm  is  guaranteed  by  the  use 
of  an  adaptive  scheme  that  determines  the  planning  and  execution  horizons.  Figure  2  shows  the 
application  of  the  theory  to  a  general  case  radius  profile  path. 


(a)  (b) 

Figure  2:  (a)  A  general  case  radius  profile  path;  (b)  The  corresponding  optimal  velocity  profile, 
along  with  the  free-boundary  conditions  problem  solutions  for  constant  radius  and  min  R  subarcs 
(from  [2]). 

Extensions  of  the  point  mass  methodology  to  a  half-car  model  are  presented  in  [7]  in  order  to  re¬ 
cover  the  missing  attitude  (yaw)  information.  The  acceleration  limits  (GG-diagram)  of  the  half-car 
model  is  determined  by  the  available  tire  friction  forces  in  the  front  and  rear  axles.  We  have  devel¬ 
oped  three  extensions  of  the  point  mass  methodology  to  the  half-car  model.  In  the  first  extension 
we  directly  implemented  the  optimal  control  strategy  of  the  point  mass  case  to  the  half-car  model. 
In  the  second  extension  the  optimal  control  strategy  of  the  point  mass  case  is  interrupted  by  a 
stabilizing  control  logic  when  the  vehicle  slip  angle  increases  beyond  a  certain  value  and  the  yaw 
dynamics  tend  to  instability.  Finally,  in  the  third  approach  we  enforce  the  additional  constraint 
that  the  vehicle  tracks  the  path  with  zero  slip  angle  and  determine  the  acceptable  acceleration 
limits  subject  to  the  new  constraint. 

In  [1,  9]  we  initiated  a  mathematical  analysis  of  rally  racing  techniques.  We  provided  an  em¬ 
pirical  description  of  Trail-Braking  (TB)  and  Pendulum-Turn  (PT)  cornering,  two  of  the  most 
common  rally  racing  maneuvers.  We  introduced  a  low  order  vehicle  model  that  can  be  efficiently 
used  within  an  optimization  scheme.  The  model  incorporates  the  appropriate  level  of  detail  to 
reproduce  modes  of  operation  typical  of  those  encountered  in  rally,  off-road  racing.  We  have  used  a 
numerical  scheme  to  study  different  trajectory  optimization  scenarios  during  cornering.  We  finally 
showed  that  our  modeling  approach  is  capable  of  reproducing  TB  and  PT  maneuvers  as  special 
cases  of  the  minimum-time  solution  with  additional  constraints.  In  [8]  we  showed  that  a  simple 
parametrization  of  the  driver’s  control  inputs  can  be  used  to  reproduce  these  maneuvers  using  a 
high  fidelity  full-car  model.  Figure  3  depicts  the  input  parameterization  used  for  the  TB  maneuver. 
The  same  parameterization  is  valid,  in  fact,  for  a  large  range  of  corner  geometries. 

These  numerical  results  were  validated  against  experimental  data  (vehicles  driven  by  professional 
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(a)  (b)  (c) 


Figure  3:  Piecewise  linear  parameterization  for  the  steering,  braking  and  throttle  inputs  for 
Trail-Braking. 


rally  racing  drivers)  collected  at  the  facilities  of  Vehicle  Control  Center  at  Dalton,  NH  and  Ford’s 
Michigan  Proving  Grounds  (MPG). 

In  [11]  we  relaxed  the  final  positioning  of  the  vehicle  with  respect  to  the  width  of  the  road  in 
order  to  study  the  optimality  of  late-apex  trajectories,  typically  followed  by  rally  drivers.  Again, 
it  was  shown  that  simple  piecewise  linear  approximations  of  the  optimal  input  profiles  suffice  to 
generate  close  to  optimal  TB  maneuvers  for  a  large  variety  of  corner  geometries  (see  Figure  4). 


(b)  90  deg  (d)  180  deg 


Figure  4:  Trail-Braking  through  the  60,  90,  135,  and  180  deg  corners.  Results  obtained  via 
numerical  optimization.  For  details  please  refer  to  [1,  11]. 
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Finally,  in  [3,  13]  we  investigated  the  existence  and  stability  properties  of  steady-state  (equilib¬ 
rium)  cornering  conditions  for  a  single-track  vehicle  model  without  imposing  restrictive  conditions 
on  tire  slip.  Steady-state  cornering  is  defined  as  cornering  along  a  path  of  constant  curvature,  with 
constant  speed  and  sideslip  angle.  The  steady-state  yaw  rate  is  fixed  as  a  function  of  speed  and  cor¬ 
ner  curvature.  In  contrast  to  traditional,  neutral  steering  (zero  vehicle  slip  angle),  we  have  shown 
that  there  indeed  exist  multiple  equilibria,  many  of  which  involve  a  nonzero  (and  often  large)  vehicle 
slip  angle.  This  provides  a  mathematical  justification  of  the  sustained  “drift”  cornering  maneuvers 
used  by  expert  race/rally  drivers.  For  each  steady-state  cornering  condition  we  calculated  the  cor¬ 
responding  tire  friction  forces  at  the  front  and  rear  tires,  as  well  as  the  required  front  steering  angle 
and  front  and  rear  wheel  slip  ratios,  to  maintain  constant  velocity,  turning  rate  and  vehicle  sideslip 
angle.  We  analyzed  the  resulting  conditions,  identifying  stable  and  unstable  relative  equilibria. 
Inspired  by  recent  progress  in  the  understanding  of  advanced  driving  techniques,  we  proposed  a 
sliding-nrode  control  scheme  stabilizing  steady-state  cornering  conditions,  using  only  longitudinal 
control  inputs,  i.e,  accelerating/braking  torques  applied  at  the  front  and/or  rear  wheels.  This  is  akin 
to  the  well-known  “left  foot  braking”  technique  used  by  rally  drivers  to  regulate  the  friction  forces  at 
the  front  and  rear  axles  by  taking  advantage  of  the  longitudinal  weight  transfer  of  the  vehicle.  Fig¬ 
ure  5  shows  the  result  from  a  rather  extreme  case  of  sustained  cornering  at  a  slip  angle  of  (3  ~  50  deg. 
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Figure  5:  An  example  of  steady-state  (sustained)  cornering  at  large  vehicle  slip  angles.  These 
equilibria,  whose  existence  is  shown  in  [13,  3],  can  be  used  to  generate  “on-the-fly”  aggressive 
cornering  maneuvers  for  autonomous  vehicles. 

2.2  Hierarchical  Multi-resolution  Path  Planning 

A  common  method  for  solving  path  planning  problems  based  on  rectangular  cell  approximations 
of  the  environment  is  to  employ  a  graph  representation.  However,  the  dimension  of  the  graph 
becomes  very  large  as  the  fidelity  of  a  global  approximation  of  the  environment  increases.  In  fact, 
it  has  been  shown  in  the  literature  that  path-planning  in  the  presence  of  obstacles  is  an  NP-hard 
problem.  In  order  to  reduce  the  computational  complexity  of  the  problem,  we  have  introduced 
a  hybrid  local/global  path  planning  algorithm  that  uses  district  levels  of  fidelity  (resolution)  of 
the  environment  at  different  distances  from  the  agent’s  current  position.  The  motivation  for  this 
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approach  is  simple:  first,  the  agent’s  immediate  reaction  to  an  obstacle  or  a  threat  is  needed  only 
at  the  vicinity  of  its  current  position.  Far  away  obstacles  or  threats  do  not  (or  should  not)  have  a 
large  effect  of  the  vehicle’s  immediate  motion.  Therefore,  the  most  accurate  and  reliable  informa¬ 
tion  is  required  at  the  vicinity  of  the  vehicle.  Second,  the  plethora  of  sensory  information  used  by 
typical  robotic  vehicles  (e.g.,  cameras,  radars,  laser  scanners,  satellite  imagery)  do  have  different 
ranges  and  resolutions.  A  computationally  efficient  path  planning  algorithm  should  be  able  to 
blend  together  the  information  provided  by  all  these  sensors  and  focus  its  computational  resources 
on  the  part  of  the  path  (spatial  and  temporal)  that  needs  it  most.  In  a  nutshell,  a  computationally 
efficient  algorithm  suitable  for  on-line  implementation  should  be  able  to  combine  short-term  tactics 
(reaction  to  unforeseen  threats)  with  long-term  strategy  (planning  towards  the  ultimate  goal). 

In  [14,  16]  we  used  wavelets  to  obtain  multiresolution  approximations  of  the  configuration  space 
at  different  distances  from  the  vehicle.  The  wavelet  transform  provides  a  very  fast  decomposition1 
of  the  environment  at  different  levels  of  resolution.  The  number  of  resolution  levels,  their  scale, 
and  range  can  all  be  readily  adapted  at  each  time  step  to  yield  graph  representations  that  are 
commensurate  to  the  available  on-board  computational  resources. 

Given  a  function  /  6  £2(M2)  we  can  then  write  its  2D  wavelet  decomposition  as  follows 

3  Jmax  1 

/  {x,  y)  ~  ^2  ajmin,M  $  Jmin,kAXl  y )  +  EE  E  (i) 

/c,^EX(./min)  i=l  j  =  Jmin  k£K,(j) 

eec(j) 


where 


$j,k/(x,y)  =  <l>j,k{x)<l>j,t{y)i  ^),k,e(xiy) =  (l)j,k(x)',Pj,e(y)i  *  =  1,2,3  (2) 

with  (l)j,k(x )  =  2j/2</>(2jx  —  k )  and  ipjtk  =  2-?/2V’(2:,x  —  k)  and  4>(x)  (scaling  function)  and  x ) 
(mother  wavelet)  have  compact  support  or  they  decay  very  fast  outside  a  small  interval  so  they  can 
capture  localized  features  of  /.  For  the  case  of  orthonormal  wavelets  the  approximation  coefficients 
are  given  by2 

/OO  /*oo 

/  f{x,y)  ®j,kAx’y)dxdy  (3) 

-oo  J  —  oo 

and  the  detail  coefficients  by 

/oo  /*oo 

/  f(x,y)^)k/(x,y)dxdy.  (4) 

-oo  J — OO 


The  key  property  of  wavelets  used  here  is  the  fact  that  the  expansion  (1)  induces  the  following 
decomposition  of  £2(M2) 

£2(M2)  =  Vj  ©  Wftail  ©  Wflf  ©  •  •  •  (5) 

where  Vj  =  spa and  similarly  y\)fetai1  =  span{'F1  k ^  dt2j  k  e,  d>3j  k, e}k,eez  for  j  >  J.  If 
we  use  Haar  wavelets,  each  scaling  function  4>jyk{ x)  and  wavelet  function  '0j,fc( x)  in  the  Haar  system 
is  supported  on  the  dyadic  interval  Ijtk  =  [k/W ,  (k  +  1)/2J]  of  length  1/2J  and  does  not  vanish  in 
this  interval.  Subsequently,  we  may  associate  the  functions  ^j,k,e  and  4©  k  ^  (i  =  1,2,3)  with  the 

rectangular  cell  =  Ij ^  x  Ij,e- 

1The  computational  complexity  of  the  wavelet  transform  is  of  order  0(n)  where  n  is  the  input  data.  This  is 
better  even  than  the  Fast  Fourier  Transform  which  has  complexity  of  order  0(n  log2  n). 

2In  the  more  general  case  of  biorthogonal  wavelets  projections  on  the  space  spanned  by  the  dual  wavelets  and 
dual  scaling  functions  should  be  used  in  (3)  and  (4). 
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(a)  tmt 5 


(b)  t  =  t2 1 


(c)  t  =  tf 


Figure  6:  Path  evolution  and  replanning.  Figures  on  the  left  show  the  multiresolution  approxima¬ 
tion  of  the  environment  with  respect  to  the  current  location  of  the  agent. 


In  particular,  the  indices  of  the  nonzero  scaling  and  wavelet  coefficients  encode  the  necessary 
information  to  construct  the  corresponding  graph  of  the  adjacency  relationships  of  all  cells.  Most 
importantly,  the  dimension  of  this  graph  can  be  completely  tailored  a  priori  by  choosing  the 
minimum  and  maximum  resolution  levels  Jmin  and  Jmax  and  the  ranges  (described  by  the  allowable 
values  of  the  spatial  indices  k  and  £  in  (1)  for  each  resolution  level  Jm;n  <  j  <  Jmax-  Figure  6  shows 
an  example  from  the  application  of  the  algorithm  to  a  path-planning  problem  using  real  terrain  data. 
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Table  1:  Sample  results  showing  the  computational  benefits  of  local  replanning. 


Image  size  (pixels) 

Execution  time  ratio 

Cost  difference  (%) 

1. 

64  x  64 

8.46 

7.17 

2. 

64  x  64 

10.7 

34.5 

3. 

64  x  64 

9.04 

16.8 

3. 

128  x  128 

18.5 

16.4 

4. 

128  x  128 

15.4 

7.95 

5. 

128  x  128 

10.6 

10.5 

6. 

256  x  256 

51.5 

2.87 

7. 

256  x  256 

51.2 

5.52 

8. 

256  x  256 

34.1 

18.8 

The  algorithm  can  also  be  used  in  case  of  moving  obstacles  and  pop-up  threats  since  replanning  is 
performed  at  each  step  the  vehicle  moves  to  a  new  location.  However,  the  algorithm  performs  a  new 
cell  decomposition  at  each  iteration.  Thus,  it  discards  all  information  about  the  path  other  than  the 
location  of  the  immediately  next  cell.  In  [15]  we  developed  a  refinement  of  the  baseline  algorithm 
based  on  local  replanning  ideas  that  explores  the  information  from  the  previous  iteration.  Table  1 
shows  sample  comparison  results  between  the  proposed  algorithm  and  that  in  [14].  It  is  seen  that 
the  local  replanning  algorithm  is  an  order  of  a  magnitude  faster  but  produces  paths  with  slightly 
higher  costs.  Also  note  that  the  savings  in  execution  time  increases  with  the  size  of  the  image  being 
processed.  The  image  sizes  are  in  pixels,  and  the  cost  difference  is  the  extra  cost  of  the  proposed 
algorithm’s  resultant  path  as  a  percentage  of  the  cost  of  path  resulting  from  the  algorithm  in  [14]. 
The  suboptinrality  of  the  path  with  local  replanning  is  typically  at  the  order  of  10%.  Case  2  in  Table 
1  is  an  anomalous  result  where  the  true  optimal  path  lied  mostly  outside  the  initial  sequence  of  cells. 

Another  potential  problem  with  the  baseline  path-planning  algorithms  stems  from  the  fact  that 
the  rectangular  cell  decomposition  using  Haar  wavelets  is  not  optimal  for  resolving  boundaries  of 
obstacles  (see  Figure  7)  using  onboard  sensors  (cameras,  radars,  antennas,  etc)  whose  area  of  influ¬ 
ence  or  field  of  view  does  not  conform  to  the  box-like  topology.  In  fact,  most  typical  sensor  devices 
provide  sector- like  representations  of  the  environment.  In  [17]  we  overcome  this  difficulty  by  em¬ 
ploying  a  conformal  mapping  to  map  the  sector  cells  to  rectangular  cells  in  a  new  coordinate  system. 
Recall  that  the  polar  transformation  maps  a  sector  specified  by  the  radii  pm in  and  pmax  and  the 
angles  0m;n  and  #max  from  the  cartesian  (x,  y)  plane  to  the  polar  plane  thus  obtaining  a  rectangular 
domain  in  the  (p,  0)  plane.  Figure  8,  taken  from  [17],  demonstrates  the  result  of  this  approach. 

2.3  Consistent  Kinodynamic  Motion  Planning  for  Autonomous  Vehicles 

A  common  approach  to  reduce  the  computational  complexity  of  the  motion-planning  problem  is 
to  divide  it  into  a  geometric  and  a  motion-planning  layers.  This  decoupling  may  lead  to  infeasi¬ 
ble/unrealistic  paths,  which  cannot  be  executed  by  the  vehicle.  In  [20]  we  propose  a  method  to 
close  this  gap  and  use  dynamic  information  (in  the  form  of  the  maximum  allowable  local  curvature 
the  vehicle  can  negotiate)  at  the  geometric  path  planning  layer.  Specifically,  our  approach  modifies 
the  well-known  Dijkstra  or  A*  algorithms  to  include  the  history  of  visited  cells  in  the  search.  The 
idea  is  rather  simple,  although  its  implementation  can  be  quite  involved.  Standard  graph-search 
based  algorithms  look,  at  each  step,  at  the  transition  cost  between  only  two  successive  nodes  (i.e., 
nodes).  This  is  too  restrictive  in  order  to  incorporate  kinematic  constraints  such  as  maximum  (lo¬ 
cal)  allowable  curvature.  Therefore,  and  in  order  to  incorporate  path  curvature  information  early 
on  (that  is,  at  the  geometric  level)  while  solving  the  motion  planning  problem,  we  work  with  cost 
functions  based  on  k— tuples  of  nodes,  for  some  fixed  k  >  2,  such  that  the  elements  of  each  A;— tuple 
are  pairwise  adjacent.  The  question  of  feasibility  of  traversal  through  k— tuples  of  cells  (rather  than 
traversal  through  two  successive  cells  only)  allows  for  more  general  definitions  of  cost  functions. 
In  particular,  we  can  introduce  transition  costs  that  capture  the  maximum  approximate  curvature 
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Figure  7:  A  cell  decomposition  based  on  the  available  sector  approximation  of  the  environment 
obtained  by  the  on-board  sensor  devices  of  the  agent  (denoted  with  the  blue  dot).  In  order  to 
resolve  the  geometry  of  the  arc-boundary  of  each  sector  the  standard  quadtree  algorithm  generates 
a  large  number  of  cells  close  to  the  boundaries  of  these  arcs. 


for  any  path  lying  inside  the  channel.  As  a  result,  we  can  ensure  that  a  feasible  trajectory  will 
always  exist  inside  the  computed  channel  of  cells  at  the  geometric,  path-planning  layer,  even  before 
invoking  the  motion-planning  task. 

This  result  is  of  independent,  theoretical  interest  and  can  be  applied  to  any  graph-search  problem. 
The  algorithm  can  be  used  anytime  a  “reasonable”  cost  for  a  sequence  of  cells  is  known. 

One  possible  instance  of  this  idea  is  to  associate  certain  sequences  of  cells  as  “good”  while  all 
others  are  deemed  undesirable.  Figure  9  shows  an  example  of  sequences  of  cells  (i.e. ,  tiles)  inside 
a  channel  that  may  be  assigned  a  good  score.  The  algorithm  penalizes  the  sought-after  path  de¬ 
pending  on  the  number  of  good  of  bad  sequences  of  tiles  it  contains.  A  much  more  sophisticated 
manner  to  construct  good  channels  with  the  simultaneous  construction  of  the  corresponding  path 
of  given  bounded  curvature  is  reported  in  [18]. 

Numerical  examples  show  that  our  algorithm  outperforms  the  naive  alternative  of  performing  the 
search  on  the  “lifted”  graph  that  contains  all  possible  combinations  of  sequences  of  cells.  Table  2 
shows,  for  several  sample  test  cases,  the  ratios  of  the  total  computational  times  between  the  execu¬ 
tion  of  Dijkstra’s  algorithm  on  the  “lifted”  graph  over  the  execution  times  of  the  proposed  algorithm. 

Another  approach  to  construct  bounded  curvature  (and  bounded  curvature  gradient)  paths  inside 
a  given  channel  has  been  reported  in  [19,  21].  Specifically,  in  [19]  we  propose  an  algorithm  for 
concatenating  arcs  of  circles  of  maximum  curvature  and  line  segments  (i.e.,  Dubins’  path  primitives) 
using  a  minimum  look-ahead  distance  (just  three  cells).  Unfortunately,  direct  concatenation  of  line 
segments  and  circle  arcs  will  result  in  a  discontinuity  in  the  path  curvature.  Tracking  such  a  curve 
will  lead  to  poor  performance.  The  main  source  of  this  poor  performance  is  the  latency  associated 
to  the  steering  command  inputs  of  typical  ground  vehicles.  It  is  therefore  of  interest,  at  least  for 
ground  wheeled  vehicle  applications,  to  consider  paths  which  not  only  have  a  curvature  bounded  by 
a  given  upper  bound,  but  also  have  a  bound  on  the  gradient  of  the  curvature.  In  [21]  we  solve  exactly 
this  problem,  by  adding  one  more  element  in  the  family  of  path  primitives:  segments  of  clothoids. 
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Table  2:  Comparison  of  execution  times  between  the  proposed  algorithm  and  the  standard  Dijkstra 
algorithm  run  on  the  “lifted”  graph. 


\Q\ 

H 

Avg.  time  ratio 

\Q\ 

H 

Avg.  time  ratio 

100 

1 

1.1472 

100 

3 

1.475 

400 

1 

1.3680 

225 

3 

1.531 

1600 

1 

1.7551 

400 

3 

1.536 

100 

2 

1.8176 

36 

4 

2.544 

400 

2 

2.3000 

64 

4 

2.519 

900 

2 

3.8407 

100 

4 

2.849 

3  Collaborations  and  Technology  Transfer 

We  established  a  close  collaboration  with  Vehicle  Control  Training,  LLC  (POC:  Greg  McKinney),  a 
company  that  specializes  in  training  military  and  government  security  personnel  in  high-speed  driv¬ 
ing  techniques,  especially  over  rough  terrain.  The  PI  has  been  a  leading  member  of  the  SAVE  (Syn¬ 
thetic  Automotive  Virtual  Environment)  program,  recently  approved  by  the  US  congress  ($1.6M  for 
FY08  and  $2.2M  for  FY09)  for  the  development  of  high-fidelity  training  driving  platforms  for  train¬ 
ing  soldiers  in  high  speed  and  abnormal  driving  conditions.  Except  VCT  and  Georgia  Tech,  other 
members  of  the  SAVE  program  team  include  Vehicle  Control  Training,  MIT,  US  Army  ERDC-Cold 
Regions  Research  &  Engineering  Laboratory,  and  SimCraft). 

Realistic  experimental  data  for  validating  the  models  describing  the  aggressive  cornering  maneu¬ 
vers  investigated  as  part  of  this  research  were  collected  through  our  established  collaborations  with 
VCT  and  Ford  Motor  Company.  Ford  (POC:  Jianbo  Lu)  provided  the  test  vehicles  and  allowed  use 
of  their  test  facilities  (Michigan  Proving  Ground)  for  data  collection  during  the  summer  of  2006. 

Ford’s  interest  in  technical  driving  techniques  stems  from  their  potential  application  in  increasing 
performance  of  current  active  vehicle  safety  systems.  This  has  led  to  a  collaboration  with  the  Pi’s 
lab  for  the  development  of  active  safety  control  algorithms  for  commercial  vehicles  using  these  ad¬ 
vanced  driving  techniques.  Two  provisional  patents  have  been  filled  as  a  result  of  this  collaboration 
with  Ford. 
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(a)  t  =  4 


(b)  t  =  10 


(c)  t  =  22 

Figure  8:  Path  evolution  with  time.  The  figures  show  the  actual  path  (solid  line)  along  with  the 
most  recent  tentative  path  (dotted  line)  of  the  agent  at  each  time  step.  The  agent  reaches  the 
final  destination  at  t  =  tf. 
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Figure  9:  Illustration  of  tiles  based  on  histories  of  three  cells. 
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